#include "movement.h"
#include "math_core.h"
#include <math.h>

#include "graphics/graphics_util.h"
#include "util/common_macros.h"


using namespace math;

Movement::SCREEN_MODE Movement::m_ScreenMode = SCREEN_MODE_UNSETYET;

Movement::Movement(float	MoveSpeed , const math::Vec2& Pos) :
						m_MoveSpeed(MoveSpeed),
						m_CurrentPos(Pos),
						m_TargetPos(Pos),
						m_EnableSnap(true),						
						m_StopAtTarget(false),
						m_IsRestrianed(false)
{
	
	if(m_ScreenMode == SCREEN_MODE_UNSETYET)
	{
		if( GraphicsUtil::WIDTH == GraphicsUtil::HEIGHT)
		{
			m_ScreenMode = SCREEN_MODE_SQUARE;
		}
		else if( GraphicsUtil::WIDTH > GraphicsUtil::HEIGHT)
		{
			m_ScreenMode = SCREEN_MODE_LANDSCAPE;
		}
		else if( GraphicsUtil::WIDTH < GraphicsUtil::HEIGHT)
		{
			m_ScreenMode = SCREEN_MODE_PORTRAIT;
		}
	}
	

}

Movement::~Movement()
{
	ClearForces();
}

void	Movement::ClearForces()
{
	for(int i = 0; i < (int)m_Forces.size(); i++)
	{
		if(m_Forces[i])
			UTIL_SAFE_DELETE(m_Forces[i]);
	}
	
	m_Forces.resize(0);
}

void	Movement::SetMinMax(const math::Vec2& Min,const math::Vec2& Max )
{
	m_Min = Min;
	m_Max = Max;
	ChangeToSquareVec2(m_Min);
	ChangeToSquareVec2(m_Max);
	m_IsRestrianed = true;
}

void	Movement::Tick(float dt)
{	

	TickForces(dt);

	if(!m_IsMoving)
	{
		m_SquarePos  += m_ForceMovement;
		m_CurrentPos = m_SquarePos;
		ChangeToScreenPos(m_CurrentPos);
		return;
	}

	m_PreviousPos = m_CurrentPos;

	float	MoveSpeed = dt * m_MoveSpeed;
	math::Vec2 PreviousPos = m_SquarePos;	
	math::Vec2 Delta = (m_SquareDir * MoveSpeed);		

    m_SquarePos += Delta;
	
	// don't force too much where I m going , otherwise it may look like movespeed is too much
	if(Delta.x > 0 && m_ForceMovement.x > 0 )
		m_ForceMovement.x = 0;
	else if(Delta.x < 0 && m_ForceMovement.x < 0 )
		m_ForceMovement.x = 0;
	if(Delta.y > 0 && m_ForceMovement.y > 0 )
		m_ForceMovement.y = 0;
	else if(Delta.y < 0 && m_ForceMovement.y < 0 )
		m_ForceMovement.y = 0;

	m_SquarePos  += m_ForceMovement;

	bool Reach = false;
	bool GotRestrianed = false;
	if(m_IsRestrianed)
	{
		if(m_SquarePos.x < m_Min.x)
		{
			m_SquarePos.x = m_Min.x;
			Reach = true;
			GotRestrianed = true;
		}
		else if(m_SquarePos.x > m_Max.x)
		{
			m_SquarePos.x = m_Max.x;
			Reach = true;
			GotRestrianed = true;
		}
		if(m_SquarePos.y < m_Min.y)
		{
			m_SquarePos.y = m_Min.y;
			Reach = true;
			GotRestrianed = true;
		}
		else if(m_SquarePos.y > m_Max.y)
		{
			m_SquarePos.y = m_Max.y;
			Reach = true;
			GotRestrianed = true;
		}
	}

	if(m_StopAtTarget)
	{
		math::Vec2 NewDir = m_SquareTarget - m_SquarePos;
		NewDir.Normalize();

		if(NewDir.x > 0 && m_SquareDir.x <= 0)
			Reach = true;

		else if(NewDir.x < 0 && m_SquareDir.x >= 0)
			Reach = true;

		else if(NewDir.y > 0 && m_SquareDir.y <= 0)
			Reach = true;

		else if(NewDir.y < 0 && m_SquareDir.y >= 0)
			Reach = true;
	}

	if(Reach)
	{
		if(m_StopAtTarget)
			m_IsMoving = false;
		if (m_EnableSnap && !GotRestrianed)
		{
			m_SquarePos = m_SquareTarget;
		} 
	}

	m_CurrentPos = m_SquarePos;
	ChangeToScreenPos(m_CurrentPos);

}



void	Movement::SetCurrent(const math::Vec2& Pos)
{
	m_PreviousPos = Pos;
	m_CurrentPos = m_TargetPos = Pos;
	m_SquarePos = m_SquareTarget = Pos;

	ChangeToSquareVec2(m_SquarePos);
	ChangeToSquareVec2(m_SquareTarget);	

	m_IsMoving = false;
}



void	Movement::ChangeToSquareVec2(math::Vec2& P)
{
	if(m_ScreenMode == SCREEN_MODE_LANDSCAPE)
	{	
		P.y *= GraphicsUtil::H2W;		
	}
	else if (m_ScreenMode == SCREEN_MODE_PORTRAIT)
	{
		P.x *= GraphicsUtil::W2H;		
	}
}

void	Movement::ChangeToScreenPos(math::Vec2& P)
{
	if(m_ScreenMode == SCREEN_MODE_LANDSCAPE)
	{	
		P.y *= GraphicsUtil::W2H;		
	}
	else if (m_ScreenMode == SCREEN_MODE_PORTRAIT)
	{
		P.x *= GraphicsUtil::H2W;		
	}
}

void	Movement::SetTarget(const math::Vec2& Pos, bool StopAtTarget)
{
	m_StopAtTarget = StopAtTarget;
	m_IsMoving = true;
	m_TargetPos  = m_SquareTarget = Pos;
	
	 
	m_Dir = m_TargetPos - m_CurrentPos;

	ChangeToSquareVec2(m_SquareTarget);
	m_SquareDir = m_SquareTarget - m_SquarePos;

	m_SquareDir.Normalize();	
	
}

void	Movement::SetTarget(float Degree)
{
	m_StopAtTarget = false;
	m_IsMoving = true;
		
	m_SquareDir = m_Dir = VectorFromAngle(Degree);;
	//ChangeToSquareVec2(m_SquareDir);	
	m_SquareDir.Normalize();	
}


//math::Vec2	Movement::GetForwardW2H() const 
//{
//	//math::Vec2	Dir = VectorFromAngle(GetAngle());
//	//Dir.y *= GraphicsUtil::W2H;
//	//return Dir;
//	return math::Vec2(0,0);
//}
//
//math::Vec2	Movement::GetForward() const 
//{
//	//return VectorFromAngle(GetAngle());
//	return math::Vec2(0,0);
//}
//
//math::Vec2	Movement::GetRight() const 
//{
//	//float	RadRotation = GetAngle() * DEG_TO_RAD;
//	//return math::Vec2(	 -sinf(RadRotation+ MATH_HALF_PI),  -sinf(RadRotation+ (MATH_HALF_PI*2.0f)));
//
//	return math::Vec2(0,0);
//}

//float	Movement::GetRemainder() const
//{
//	//if ((float)fabs(m_TargetAngle - m_CurrentAngle) > 180.0f) return fabs(m_CurrentAngle-m_TargetAngle);
//	//return fabs(m_TargetAngle - m_CurrentAngle);
//	return 0;
//}


math::Vec2	Movement::VectorFromAngle(float Angle)
{
	float	RadRotation = Angle * DEG_TO_RAD;
	math::Vec2 Temp = math::Vec2(	-sinf(RadRotation), -sinf(RadRotation+ MATH_HALF_PI));
	//ChangeToSquareVec2(Temp);
	return Temp;
}

void	Movement::AddForce(const math::Vec2& Dir,  float Duration, float Acceleration)
{	
	//return;
	//m_Forces.push_back(pForce);
	for (int i = 0; i < (int)m_Forces.size(); i++)
	{
		Movement::Force* pForce = m_Forces[i];
		if(pForce->m_IsFinish)
		{
			pForce->m_Dir = Dir;
			pForce->m_Duration = Duration;
			pForce->m_Acceleration = Acceleration;
			pForce->m_IsFinish = false;	
			return;
		}
	}
	
	Movement::Force* pForce = new Movement::Force();
	pForce->m_Dir = Dir;
	pForce->m_Duration = Duration;
	pForce->m_Acceleration = Acceleration;
	pForce->m_IsFinish = false;	
	m_Forces.push_back(pForce);
	
}

void	Movement::TickForces(float dt)
{
	m_ForceMovement.x = 0;
	m_ForceMovement.y = 0;
	/*
	// delete that have done.
	for (int i = (int)(m_Forces.size() - 1); i >= 0; i--)
	{
		WDSEnemy::Force* pForce = m_Forces[i];
		if (pForce->m_CurrentValue > pForce->m_TargetValue)
		{
			UTIL_SAFE_DELETE(m_Forces[i]);
			m_Forces.erase(m_Forces.begin() + i);			
		}
	}
	*/

	//m_CurrentPos = m_SquarePos;
	//ChangeToScreenPos(m_CurrentPos);	

	for (int i = 0; i < (int)m_Forces.size(); i++)
	{
		Movement::Force* pForce = m_Forces[i];	
		if(pForce->m_IsFinish == false)
		{
			m_ForceMovement +=  (pForce->m_Dir * pForce->m_Acceleration * dt);
			pForce->m_Acceleration -= dt;
			if(pForce->m_Acceleration <= 0)
				pForce->m_Acceleration = 0;

			pForce->m_Duration -= dt;
			if(pForce->m_Duration <= 0)
				pForce->m_IsFinish = true;
		}
		//m_Movement.SetCurrent(
	}

	//m_SquarePos = m_CurrentPos;	
	//ChangeToSquareVec2(m_SquarePos);
	ChangeToSquareVec2(m_ForceMovement);
}

